﻿// CameraSensorParameters
// Container for CameraSensor parameters
// 
// FLMScan - Featured by FabLab Munich

using System;
using System.IO;
using System.Xml;
using System.Globalization;
using log4net;

namespace FLMScan.Parameters
{
    public class CameraSensorParameters
    {
        private ILog logger;

        private const double DefaultSensorWidth = 4.536;
        private const double DefaultSensorHeight = 3.416;
        private const int DefaultResolutionX = 1600;
        private const int DefaultResolutionY = 1200;
        private const double DefaultFocalLengthInPixelsX = 1361.2218516055927;
        private const double DefaultFocalLengthInPixelsY = 1353.5809486568312;
        private const double DefaultPrimaryPointInPixelsX = 799.5;
        private const double DefaultPrimaryPointInPixelsY = 599.5;
        private const double DefaultDistortionCoefficient1 = 0.029887035827730758;
        private const double DefaultDistortionCoefficient2 = -0.15268759142955779;
        private const double DefaultDistortionCoefficient3 = 0.0061148104689806131;
        private const double DefaultDistortionCoefficient4 = 0.0085850554542710126;

        private double sensorWidth;
        private double sensorHeight;
        private int resolutionX;
        private int resolutionY;
        private double focalLengthInPixelsX;
        private double focalLengthInPixelsY;
        private double primaryPointInPixelsX;
        private double primaryPointInPixelsY;
        private double distortionCoefficient1;
        private double distortionCoefficient2;
        private double distortionCoefficient3;
        private double distortionCoefficient4;

        private const String SensorWidthParameter = "SensorWidth";
        private const String SensorHeightParameter = "SensorHeight";
        private const String SensorResolutionXParameter = "SensorResolutionX";
        private const String SensorResolutionYParameter = "SensorResolutionY";
        private const String CameraFocalLengthInPixelsXParameter = "CameraFocalLengthInPixelsX";
        private const String CameraFocalLengthInPixelsYParameter = "CameraFocalLengthInPixelsY";
        private const String CameraPrimaryPointInPixelsXParameter = "CameraPrimaryPointInPixelsX";
        private const String CameraPrimaryPointInPixelsYParameter = "CameraPrimaryPointInPixelsY";
        private const String DistortionCoefficient1Parameter = "DistortionCoefficient1";
        private const String DistortionCoefficient2Parameter = "DistortionCoefficient2";
        private const String DistortionCoefficient3Parameter = "DistortionCoefficient3";
        private const String DistortionCoefficient4Parameter = "DistortionCoefficient4";        

        public CameraSensorParameters()
        {
            logger = LogManager.GetLogger(typeof(CameraSensorParameters));

            SetDefaultValues();
        }

        public CameraSensorParameters(string configurationFilePath)
        {
            logger = LogManager.GetLogger(typeof(CameraSensorParameters));

            Initialize(configurationFilePath);            
        }
                        
        public void Serialize(XmlTextWriter textWriter)
        {
            FLMScanParameters.SerializeParameter(textWriter, SensorWidthParameter, SensorWidth.ToString());
            FLMScanParameters.SerializeParameter(textWriter, SensorHeightParameter, SensorHeight.ToString());
            FLMScanParameters.SerializeParameter(textWriter, SensorResolutionXParameter, ResolutionX.ToString());
            FLMScanParameters.SerializeParameter(textWriter, SensorResolutionYParameter, ResolutionY.ToString());
            FLMScanParameters.SerializeParameter(textWriter, CameraFocalLengthInPixelsXParameter, FocalLengthInPixelsX.ToString());
            FLMScanParameters.SerializeParameter(textWriter, CameraFocalLengthInPixelsYParameter, FocalLengthInPixelsY.ToString());
            FLMScanParameters.SerializeParameter(textWriter, CameraPrimaryPointInPixelsXParameter, PrimaryPointInPixelsX.ToString());
            FLMScanParameters.SerializeParameter(textWriter, CameraPrimaryPointInPixelsYParameter, PrimaryPointInPixelsY.ToString());

            FLMScanParameters.SerializeParameter(textWriter, DistortionCoefficient1Parameter, DistortionCoefficient1.ToString());
            FLMScanParameters.SerializeParameter(textWriter, DistortionCoefficient2Parameter, DistortionCoefficient2.ToString());
            FLMScanParameters.SerializeParameter(textWriter, DistortionCoefficient3Parameter, DistortionCoefficient3.ToString());
            FLMScanParameters.SerializeParameter(textWriter, DistortionCoefficient4Parameter, DistortionCoefficient4.ToString());            
        }

        private void Initialize(string configFilePath)
        {
            SetDefaultValues();

            try
            {
                TextReader reader = new StreamReader(configFilePath);
                GetConfigValues(reader);
            }
            catch (FileNotFoundException)
            {

            }
        }

        private void SetDefaultValues()
        {
            sensorWidth = DefaultSensorWidth;
            sensorHeight = DefaultSensorHeight;
            resolutionX = DefaultResolutionX;
            resolutionY = DefaultResolutionY;
            focalLengthInPixelsX = DefaultFocalLengthInPixelsX;
            focalLengthInPixelsY = DefaultFocalLengthInPixelsY;
            primaryPointInPixelsX = DefaultPrimaryPointInPixelsX;
            primaryPointInPixelsY = DefaultPrimaryPointInPixelsY;
            distortionCoefficient1 = DefaultDistortionCoefficient1;
            distortionCoefficient2 = DefaultDistortionCoefficient2;
            distortionCoefficient3 = DefaultDistortionCoefficient3;
            distortionCoefficient4 = DefaultDistortionCoefficient4;
        }        
        
        private void GetConfigValues(TextReader reader)
        {
            XmlTextReader xmlReader = new XmlTextReader(reader);
            while (xmlReader.Read())
            {
                if (xmlReader.NodeType == XmlNodeType.Element &&
                    xmlReader.Name.Equals("add") &&
                    xmlReader.MoveToNextAttribute())
                {
                    try
                    {
                        switch (xmlReader.Name)
                        {
                            case SensorWidthParameter:
                                string value = xmlReader.Value;
                                string ensureDecimalPoints = value.Replace('.', ',');
                                SensorWidth = double.Parse(ensureDecimalPoints, NumberStyles.Float);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, SensorWidthParameter, SensorWidth);
                                break;
                            case SensorHeightParameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                SensorHeight = double.Parse(ensureDecimalPoints, NumberStyles.Float);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, SensorHeightParameter, SensorHeight);
                                break;
                            case SensorResolutionXParameter:
                                ResolutionX = int.Parse(xmlReader.Value);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, SensorResolutionXParameter, ResolutionX);
                                break;
                            case SensorResolutionYParameter:
                                ResolutionY = int.Parse(xmlReader.Value);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, SensorResolutionYParameter, ResolutionY);
                                break;
                            case CameraFocalLengthInPixelsXParameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                FocalLengthInPixelsX = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, CameraFocalLengthInPixelsXParameter, FocalLengthInPixelsX);
                                break;
                            case CameraFocalLengthInPixelsYParameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                FocalLengthInPixelsY = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, CameraFocalLengthInPixelsYParameter, FocalLengthInPixelsY);
                                break;
                            case CameraPrimaryPointInPixelsXParameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                PrimaryPointInPixelsX = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, CameraPrimaryPointInPixelsXParameter, PrimaryPointInPixelsX);
                                break;
                            case CameraPrimaryPointInPixelsYParameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                PrimaryPointInPixelsY = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, CameraPrimaryPointInPixelsYParameter, PrimaryPointInPixelsY);
                                break;
                            case DistortionCoefficient1Parameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                DistortionCoefficient1 = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, DistortionCoefficient1Parameter, DistortionCoefficient1);
                                break;
                            case DistortionCoefficient2Parameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                DistortionCoefficient2 = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, DistortionCoefficient2Parameter, DistortionCoefficient2);
                                break;
                            case DistortionCoefficient3Parameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                DistortionCoefficient3 = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, DistortionCoefficient3Parameter, DistortionCoefficient3);
                                break;
                            case DistortionCoefficient4Parameter:
                                value = xmlReader.Value;
                                ensureDecimalPoints = value.Replace('.', ',');
                                DistortionCoefficient4 = double.Parse(ensureDecimalPoints, NumberStyles.Float | NumberStyles.AllowExponent);
                                logger.DebugFormat(FLMScanParameters.LogSettingParameterToValue, DistortionCoefficient4Parameter, DistortionCoefficient4);
                                break;
                        }
                    }
                    catch (System.Exception e)
                    {
                        System.Diagnostics.Debug.Assert(false, e.ToString());
                        logger.Error(FLMScanParameters.LogExceptionReadingParametersFromConfigFile, e);
                    }
                }
            }

            xmlReader.Close();
        }

        public double SensorWidth
        {
            get { return sensorWidth; }
            set { sensorWidth = value; }
        }

        public double SensorHeight
        {
            get { return sensorHeight; }
            set { sensorHeight = value; }
        }

        public int ResolutionX
        {
            get { return resolutionX; }
            set { resolutionX = value; }
        }

        public int ResolutionY
        {
            get { return resolutionY; }
            set { resolutionY = value; }
        }

        public double FocalLengthInPixelsX
        {
            get { return focalLengthInPixelsX; }
            set { focalLengthInPixelsX = value; }
        }

        public double FocalLengthInPixelsY
        {
            get { return focalLengthInPixelsY; }
            set { focalLengthInPixelsY = value; }
        }

        public double PrimaryPointInPixelsX
        {
            get { return primaryPointInPixelsX; }
            set { primaryPointInPixelsX = value; }
        }

        public double PrimaryPointInPixelsY
        {
            get { return primaryPointInPixelsY; }
            set { primaryPointInPixelsY = value; }
        }

        public double DistortionCoefficient1
        {
            get { return distortionCoefficient1; }
            set { distortionCoefficient1 = value; }
        }

        public double DistortionCoefficient2
        {
            get { return distortionCoefficient2; }
            set { distortionCoefficient2 = value; }
        }

        public double DistortionCoefficient3
        {
            get { return distortionCoefficient3; }
            set { distortionCoefficient3 = value; }
        }

        public double DistortionCoefficient4
        {
            get { return distortionCoefficient4; }
            set { distortionCoefficient4 = value; }
        }
    }
}
